#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <Eigen/Core>
#include <iostream>
using namespace std;

boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("realsense pcl"));
ros::Publisher pub;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
{
  // 检查输入点云是否为空
  if (input->data.empty()) {
    ROS_WARN("Received empty point cloud");
    return;
  }

  // 声明存储原始数据与滤波后的数据的点云的格式
  pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2); //原始的点云的数据格式
  
  // 转化为PCL中的点云的数据格式
  pcl_conversions::toPCL(*input, *cloud);

  // 转换为PointXYZRGB格式的点云
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::fromPCLPointCloud2(*cloud, *cloud_rgb);

  // 检查点云是否为空
  if (cloud_rgb->empty()) {
    ROS_WARN("Converted point cloud is empty");
    return;
  }

  // 发布点云数据
  pub.publish(*cloud);

  // 设置背景为黑色
  viewer1->setBackgroundColor(0, 0, 0);
  
  // 如果点云中没有RGB信息，则添加默认颜色
  if (cloud_rgb->size() > 0 && 
      (cloud_rgb->points[0].r != cloud_rgb->points[0].r || // 检查NaN
       (cloud_rgb->points[0].r == 0 && cloud_rgb->points[0].g == 0 && cloud_rgb->points[0].b == 0))) {
    // 如果没有有效的RGB信息，使用基于高度的颜色映射
    pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZRGB> color_handler(cloud_rgb, "z");
    if (!viewer1->updatePointCloud(cloud_rgb, color_handler, "realsense pcl")) {
      viewer1->addPointCloud(cloud_rgb, color_handler, "realsense pcl");
    }
  } else {
    // 如果有RGB信息，直接显示
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb_handler(cloud_rgb);
    if (!viewer1->updatePointCloud(cloud_rgb, rgb_handler, "realsense pcl")) {
      viewer1->addPointCloud(cloud_rgb, rgb_handler, "realsense pcl");
    }
  }

  viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "realsense pcl");
  viewer1->spinOnce(100); // 增加延迟以确保界面刷新
}

int main(int argc, char **argv)
{
  // Initialize ROS
  ros::init(argc, argv, "pcl");
  ros::NodeHandle nh;

  // 初始化可视化界面
  viewer1->setBackgroundColor(0, 0, 0);
  viewer1->addCoordinateSystem(1.0);
  viewer1->initCameraParameters();

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe("/rslidar_points", 1, cloud_cb);
  
  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<sensor_msgs::PointCloud2>("output", 1);

  // 设置循环频率
  ros::Rate loop_rate(30); // 30Hz

  while (ros::ok() && !viewer1->wasStopped()) {
    ros::spinOnce();
    viewer1->spinOnce(100);
    loop_rate.sleep();
  }

  return 0;
}
